
#include "Localization.h"
#include <stdlib.h>
#include <math.h>

//constructor
Localization::Localization
( 
uint32_t grid_size_x,
uint32_t grid_size_y, 
double square_size_x, 
double square_size_y, 
double size_dead_zone, 
double size_path_width,
double angle_offset
)
:
_grid_size_x(grid_size_x),
_grid_size_y(grid_size_y),
_square_size_x(square_size_x),
_square_size_y(square_size_y),
_size_dead_zone(size_dead_zone),
_size_path_width(size_path_width),
_angle_offset(angle_offset)
{
   for(int i=0; i < 100 ; i++)
   {
      for(int j=0; j < 100 ; j++)
      {
         _IEDSquares[i][j]._lowAngle = 0;
         _IEDSquares[i][j]._highAngle = 0;
         _IEDSquares[i][j]._lowRange = 0;
         _IEDSquares[i][j]._highRange = 0;
      }
   }  
}

//destructor
Localization::~Localization()
{
   
}

//Calling this function will update the vital information 
//for all possible IED locations
void Localization::Update
(
   double XPos,
   double YPos,
   double ZPos,
   double Heading //Radians
)
{
   double minXRange = 0;
   double minYRange = 0;
   double maxXRange = 0;
   double maxYRange = 0;
   double minRange = 0;
   double maxRange = 0;
   double directAngle = 0;
   double lowAngle = 0;
   double highAngle = 0;

   for(int i=0; i < 100 ; i++)
   {
      for(int j=0; j < 100 ; j++)
      {
         minXRange = 
            (_size_path_width + _size_dead_zone 
             + i*(_square_size_x + _size_path_width + _size_dead_zone))
            - XPos;

         minYRange = 
            (_size_path_width + _size_dead_zone  
             + j*(_square_size_y + _size_path_width + _size_dead_zone))
            - YPos;

         maxXRange = minXRange + _square_size_x - _size_dead_zone;
         maxYRange = minYRange + _square_size_y - _size_dead_zone;

         minRange = sqrt(pow(minXRange, 2) + pow(minYRange, 2));
         maxRange = sqrt(pow(maxXRange, 2) + pow(maxYRange, 2));

         //center angle of square
         directAngle = atan(minYRange/minXRange) - Heading;
         lowAngle = directAngle - directAngle*_angle_offset;
         highAngle = directAngle - directAngle*_angle_offset;


         while(lowAngle >= 2*M_PI)
         {
            lowAngle = lowAngle - 2*M_PI;
         }
         while(lowAngle < 0)
         {
            lowAngle = lowAngle + 2*M_PI;
         }

         while(highAngle >= 2*M_PI)
         {
            highAngle = highAngle - 2*M_PI;
         }
         while(highAngle < 0)
         {
            highAngle = highAngle + 2*M_PI;
         }


         _IEDSquares[i][j]._lowAngle = lowAngle;
         _IEDSquares[i][j]._highAngle = highAngle;
         _IEDSquares[i][j]._lowRange = minRange;
         _IEDSquares[i][j]._highRange = maxRange;
      }
   }  

}

//Calling this function will return information about the designated
//square coordinates
SquareInfo Localization::GetSquareInfo
(
uint32_t grid_pos_x,
uint32_t grid_pos_y
)
{
   
   return _IEDSquares[grid_pos_x][grid_pos_y];

}



